'  Servo program using On Interrupt
'  Uses TMR0 and prescaler.  Watchdog Timer should be
'  set to off at program time and Nap and Sleep should not be used.
'  Buttons may be used to set hours and minutes

' Define LOADER_USED to allow use of the boot loader.
' This will not affect normal program operation.
Define	LOADER_USED	1

Define  LCD_DREG        PORTD   ' Define LCD connections
Define  LCD_DBIT        4
Define  LCD_RSREG       PORTE
Define  LCD_RSBIT       0
Define  LCD_EREG        PORTE
Define  LCD_EBIT        1

Servo1Pos VAR	Word	' Servo 1 position
Servo2Pos VAR	Word	' Servo 2 position
ServoSelect VAR	Bit	' Servo selection

Servo1	VAR	PORTC.1	' Alias first servo pin
Servo2	VAR	PORTC.0	' Alias second servo pin


	ServoSelect = 0	' Set to first servo
	Low Servo1	' Set up servos
	Low Servo2
	Servo1Pos = 500	' Center servos
	Servo2Pos = 500


        ADCON1 = 7      ' PORTA and PORTE to digital
        Low PORTE.2     ' LCD R/W low = write
        Pause 100       ' Wait for LCD to startup
		Lcdout $fe, 1	' Clear screen

        OPTION_REG = $7f        ' Enable PORTB pullups

' Set TMR0 to interrupt every 16.384 milliseconds
        OPTION_REG = $55        ' Set TMR0 configuration and enable PORTB pullups
        INTCON = $a0            ' Enable TMR0 interrupts
        ON Interrupt GoTo timerint


' Main program loop
mainloop:
        PORTB = 0       ' PORTB lines low to read buttons
        TRISB = $fe     ' Enable first button row

        ' Check any button pressed to move servo
        IF PORTB.4 = 0 Then
                GoSub left1
        Endif
        IF PORTB.5 = 0 Then
                GoSub right1
        Endif
        IF PORTB.6 = 0 Then
                GoSub left2
        Endif
        IF PORTB.7 = 0 Then
                GoSub right2
        Endif

        Lcdout $fe, $80, "Position1 = ", #Servo1Pos, "   "
        Lcdout $fe, $c0, "Position2 = ", #Servo2Pos, "   "

        GoTo mainloop   ' Do it all forever


' Move servo 1 left
left1:	IF Servo1Pos < 1000 Then
                Servo1Pos = Servo1Pos + 1
        Endif
	Pause 10	' Debounce
        Return

' Move servo 1 right
right1:	IF Servo1Pos != 0 Then
                Servo1Pos = Servo1Pos - 1
        Endif
	Pause 10	' Debounce
        Return

' Move servo 2 left
left2:	IF Servo2Pos < 1000 Then
                Servo2Pos = Servo2Pos + 1
        Endif
	Pause 10	' Debounce
        Return

' Move servo 2 right
right2:	IF Servo2Pos != 0 Then
                Servo2Pos = Servo2Pos - 1
        Endif
	Pause 10	' Debounce
        Return


' Interrupt routine to handle timer
        disable         ' Disable interrupts during interrupt handler
timerint: IF ServoSelect Then tiservo2

	Servo1 = 1	' Do first servo
	PauseUs 1000 + Servo1Pos
	Servo1 = 0
	GoTo tiexit

tiservo2: Servo2 = 1	' Do second servo
	PauseUs 1000 + Servo2Pos
	Servo2 = 0

tiexit:	ServoSelect = ServoSelect + 1	' Point to next servo
	INTCON.2 = 0    ' Reset timer interrupt flag
        Resume

        End
